use futures::{stream::StreamExt};
use r2r::unitree_api;
use r2r::{self, QosProfile};
use std::sync::atomic::{AtomicBool, Ordering};
use std::sync::{Arc, Mutex};
use std::thread;
use std::time::Duration;
use tokio;
/**
 * Go2 Intelligent Stand Demo using SportClient
 * Subscribes to high_level_state and automatically adjusts robot posture:
 * - If standing: send StandDown command
 * - If squatting/sitting: send StandUp command  
 * - Other states: send BalanceStand command
 **/
use unitree_ros2_rs::SportClient;
use unitree_ros2_rs::sport_client::state_machine::SportState;

static RUNNING: AtomicBool = AtomicBool::new(true);

#[tokio::main]
async  fn main() -> Result<(), Box<dyn std::error::Error>> {
    // Setup Ctrl+C handler
    ctrlc::set_handler(move || {
        println!("\nReceived Ctrl+C, shutting dowRequestn...");
        RUNNING.store(false, Ordering::SeqCst);
    })?;

    println!("Starting Go2 Intelligent Stand Demo...");

    // Create ROS2 context and nodes
    let ctx = r2r::Context::create()?;
    let node = r2r::Node::create(ctx.clone(), "go2_sport_client", "")?;

    let node_arc = std::sync::Arc::new(Mutex::new(node));

    let sport_client = SportClient::from_node(node_arc.clone())?;
    println!("✓ SportClient initialized");

    let node_clone = node_arc.clone();
    // Create subscriber for high level state
    let mut state_subscriber = node_clone.lock().unwrap().subscribe::<r2r::unitree_go::msg::SportModeState>(
        "/sportmodestate",
        QosProfile::default(),
    )?;
    println!("✓ Subscribed to /sportmodestate");

    // Wait a moment for connections to establish
    thread::sleep(Duration::from_millis(1000));

    // Setup async task pool
    // let mut pool = LocalPool::new();
    // let spawner = pool.spawner();

    // Clone sport_client for async task
    let sport_client_arc = std::sync::Arc::new(Mutex::new(sport_client));
    let sport_client_clone = sport_client_arc.clone();

    // Handle state messages - execute only once
    tokio::task::
        spawn(async move {
            match state_subscriber.next().await {
                Some(msg) => {
                    handle_sport_state(&msg, &sport_client_clone).await;
                    // Set RUNNING to false after processing one message
                    RUNNING.store(false, Ordering::SeqCst);
                },
                None => {
                    eprintln!("No messages received from /sportmodestate");
                },
            }
        });

    println!("✓ Waiting for one robot state message...");

    let node_clone = node_arc.clone();
    // Main loop - will exit after processing one message
    while RUNNING.load(Ordering::SeqCst) {
        node_clone.lock().unwrap().spin_once(Duration::from_millis(100));
        thread::sleep(Duration::from_millis(10));
    }

    println!("Go2 Intelligent Stand Demo completed!");
    Ok(())
}

async  fn handle_sport_state(msg: &r2r::unitree_go::msg::SportModeState, sport_client: &Arc<Mutex<SportClient>>) {
    // Parse current sport state from error code
    let current_state = SportState::from_error_code(msg.error_code as i32);

    match current_state {
        Some(state) => {
            println!(
                "Current robot state: {:?} (error_code: {})",
                state, msg.error_code
            );

            let mut req = unitree_api::msg::Request::default();

            match state {
                // If robot is in standing states, make it squat down
                SportState::BalanceStand
                | SportState::StandLock
                | SportState::Agile
                | SportState::Classic
                | SportState::NormalWalk => {
                    println!("Robot is standing, sending StandDown command...");
                    sport_client.lock().unwrap().StandDown(&mut req);
                    // send_command(&sport_client, &req, "StandDown");
                }

                // If robot is squatting or sitting, make it stand up
                SportState::Squat | SportState::Sit | SportState::Damping => {
                    println!("Robot is down, sending StandUp command...");
                    sport_client.lock().unwrap().StandUp(&mut req);
                    // send_command(&sport_client, &req, "StandUp");
                }

                // For other states, use balance stand
                _ => {
                    println!("Robot in other state, sending BalanceStand command...");
                    sport_client.lock().unwrap().BalanceStand(&mut req);
                    // send_command(&sport_client, &req, "BalanceStand");
                }
            }
        }
        None => {
            println!("Unknown sport state with error_code: {}", msg.mode);
        }
    }
}

// fn send_command(sport_client: &SportClient, req: &Request, command_name: &str) {
//     if let Some(response) = sport_client.Call(req, Duration::from_secs(3)) {
//         println!("✓ {} command sent successfully!", command_name);
//         println!("Response: {}", response);
//     } else {
//         println!("⚠ {} command failed or timeout occurred", command_name);
//     }
//     println!("---");
// }
